/*
*
* File Name          :  Sound.c
* Description        :  Sound Functions
*/

#include <stdint.h>
#include <math.h>
#ifdef _STM32x_
#include "stm32f10x.h"
#include "circle_api.h"
#endif
#include "Sound.h"
#include "Hrtf.h"
#include "HrtfArray.h"
#include "ClockCounter.h"

//#define SINWAVE_STEREO
#define MAX_SIGNAL 32000
#define MIN_SIGNAL -32000
#define SOUND_FFT_SCALE 100

uint8_t _Sound_fEOF = 0; //FlagEndOfFile
AUDIO_PlaybackBuffer_Status _Sound_BufferStatus;
SOUND_BUFFER_TYPE _Sound_AudioBuffer[SOUND_BUFFER_LENGTH];
HRTF_StereoSignal _Sound_StereoSignal;

const int16_t _Sound_Sinwave [] =
{
#include "Sinwave.h"
};

int16_t Sound_Init(void) {
  HRTF_Init();
  return 1;
}

SOUND_BUFFER_TYPE *Sound_Get_AudioBuffer() { 
    return _Sound_AudioBuffer;
}

int32_t Sound_Get_AudioBuffer_Size() { 
    return SOUND_BUFFER_LENGTH;
}

int32_t Sound_Read(int16_t *dest, const int16_t *src, int32_t destLength, int32_t srcLength) {
    int32_t i, j, blockLength, currentPos;
    blockLength = destLength/srcLength;
    currentPos = 0;
    for(i=0; i<blockLength; i++) {
        for(j=0; j<srcLength; j++) {
            dest[currentPos] = src[j];
            currentPos++;
        } 
    }
    return currentPos;
}

float Sound_GetAzimuth() {
  static float azimuth = 0.0;
  float deltaAzimuth = 1.0;
  enum JOYSTICK_state jState;

  jState = JOYSTICK_GetState();
  if(jState==JOYSTICK_RIGHT)
    azimuth+=deltaAzimuth;
  else 
    if(jState==JOYSTICK_LEFT)
      azimuth-=deltaAzimuth;
  if(azimuth>HRTF_AZIMUTH_MAX)
    azimuth = HRTF_AZIMUTH_MAX;
  else if(azimuth<HRTF_AZIMUTH_MIN)
    azimuth = HRTF_AZIMUTH_MIN;
  
  return azimuth;
}

int32_t Sound_Position(SOUND_BUFFER_TYPE *dest, const int16_t *src, int32_t destLength, int32_t srcLength) { 
  int32_t i, currentPos;
  int32_t sig;
  float azimuth;
  char str1[16];
  
  STOPWATCH_START
  azimuth = Sound_GetAzimuth();     
  HRTF_SoundPosition(&_Sound_StereoSignal, (FPComplex *) src, srcLength, 0, azimuth);
 
  UTIL_int2str( &str1[0], (int)round(azimuth), 5, 1 );
  DRAW_DisplayString( 0, 90, str1, 6);
  
  currentPos = 0;
  for(i=0; i<srcLength; i++) {
    sig = _Sound_StereoSignal.left[i][0]*SOUND_FFT_SCALE;
    if(sig>MAX_SIGNAL)
      sig = MAX_SIGNAL;
    else 
      if(sig<MIN_SIGNAL)
        sig = MIN_SIGNAL;
    dest[currentPos] = (SOUND_BUFFER_TYPE)sig;
    currentPos++;
    
    sig = _Sound_StereoSignal.right[i][0]*SOUND_FFT_SCALE;
    if(sig>MAX_SIGNAL)
      sig = MAX_SIGNAL;
    else 
      if(sig<MIN_SIGNAL)
        sig = MIN_SIGNAL;
    dest[currentPos] = (SOUND_BUFFER_TYPE)sig;
    currentPos++;
  }
  STOPWATCH_STOP
  CycleCounter_Print(0, 130, 10);
  return currentPos;
}

enum MENU_code Sound_FillBuffer(void) {
    int32_t i;
    int32_t half_audio_buffer = SOUND_BUFFER_LENGTH/2;
    _Sound_BufferStatus = AUDIO_PlaybackBuffer_GetStatus(0);
    static int cnt0 = 0;
    static uint8_t str1[16];

    if ( _Sound_fEOF ) {
        if ( ( _Sound_BufferStatus & LOW_EMPTY ) && ( _Sound_BufferStatus & HIGH_EMPTY ) ) {
            DRAW_Clear();
            AUDIO_Playback_Stop();
            return MENU_Quit();
        }
        else 
            return MENU_CONTINUE;
    }

    TIM_Cmd( TIM2, DISABLE );

    if( _Sound_BufferStatus & LOW_EMPTY ) {
        //i = Sound_Read(_Sound_AudioBuffer, _Sound_Sinwave, half_audio_buffer, sizeof(_Sound_Sinwave)/sizeof(int16_t));
        i = Sound_Position(_Sound_AudioBuffer, _Sound_Sinwave, half_audio_buffer, DSP_FFT_SAMPLE_LENGTH);
        _Sound_BufferStatus = AUDIO_PlaybackBuffer_GetStatus(LOW_EMPTY);
        if ( i < half_audio_buffer ) {
            _Sound_fEOF  = 1;            
            for((i/=sizeof(int16_t));i<(half_audio_buffer/2);i++)
                _Sound_AudioBuffer[i] = 0;
        }
    }

    if( _Sound_BufferStatus & HIGH_EMPTY ) {
        //i = Sound_Read(_Sound_AudioBuffer+half_audio_buffer, _Sound_Sinwave, half_audio_buffer, sizeof(_Sound_Sinwave)/sizeof(int16_t));
        i = Sound_Position(_Sound_AudioBuffer+half_audio_buffer, _Sound_Sinwave, half_audio_buffer, DSP_FFT_SAMPLE_LENGTH);
        _Sound_BufferStatus = AUDIO_PlaybackBuffer_GetStatus(HIGH_EMPTY);
        if ( i< half_audio_buffer ) {
            _Sound_fEOF  = 1;                    
            for((i/=sizeof(int16_t));i<(half_audio_buffer/2);i++)
                _Sound_AudioBuffer[i+half_audio_buffer/2] = 0;
        }
    }
    
    cnt0++;
    if(cnt0%10==0) {
        UTIL_int2str( &str1[0], cnt0, 5, 1 );
        DRAW_DisplayString( 0, 60, str1, 6);
    }
    TIM_Cmd( TIM2, ENABLE );

    return MENU_CONTINUE;
}